#ifndef COOLPOOL_SIMULATION_SIMOBJECTS_RIGIDBODY_H
#define COOLPOOL_SIMULATION_SIMOBJECTS_RIGIDBODY_H

#include <MatrixTools.h>

#include "Constants.h"
#include "PointMass.h"

namespace matrix_tools = math_tools::matrix_tools;

namespace coolpool
{
	// More complicated type of dynamic object which also has a body,
	// so it is also defined by its orientation.
	class RigidBody: public PointMass
	{
	public:

		virtual ~RigidBody();

		// Overrides:
		virtual void onUpdate(double timeStep);

		virtual void clear();

		virtual void reverse();

		// Returns the velocity of a given point on rigid body.
		math_tools::Vector3D pointVelocity(math_tools::Vector3D point);

		inline void addTorque(math_tools::Vector3D torque)
		{
			torque_ += torque;
		}

		inline void saveRigidBody()
		{
			status_rigid_body[0] = angular_momentum_.getX();
			status_rigid_body[1] = angular_momentum_.getY();
			status_rigid_body[2] = angular_momentum_.getZ();
			status_rigid_body[3] = torque_.getX();
			status_rigid_body[4] = torque_.getY();
			status_rigid_body[5] = torque_.getZ();
			std::copy(&current_orientation_[0], &current_orientation_[16], &status_rigid_body[6]);
		}

		// Getters:
		inline const math_tools::Vector3D & getAngularMomentum() const
		{
			return angular_momentum_;
		}
		inline const math_tools::Vector3D getAngularVelocity() const
		{
			math_tools::Vector3D angular_velocity;
			matrix_tools::transform(inertia_tensor_inverse_, angular_momentum_, angular_velocity);
			return angular_velocity;
		}
		inline double getCoefficientOfDynamicFrictionWith(int type_id)
		{
			coeffs_iterator it = coefficients_of_dynamic_friction_.find(type_id);
			return (it != coefficients_of_dynamic_friction_.end()) ? it->second : 0.0;
		}
		inline double getCoefficientOfStaticFrictionWith(int type_id)
		{
			coeffs_iterator it = coefficients_of_static_friction_.find(type_id);
			return (it != coefficients_of_static_friction_.end()) ? it->second : 0.0;
		}
		inline double getCoefficientOfRestitionWith(int type_id)
		{
			coeffs_iterator it = coefficients_of_restitution_.find(type_id);
			return (it != coefficients_of_restitution_.end()) ? it->second : 1.0;
		}
		inline const math_tools::Matrix3D & getCurrentOrientation() const
		{
			return current_orientation_;
		}
		inline const math_tools::Matrix3D & getInertiaTensor() const
		{
			return inertia_tensor_;
		}
		inline const math_tools::Matrix3D & getInertiaTensorInverse() const
		{
			return inertia_tensor_inverse_;
		}
		inline const math_tools::Vector3D getPointRelativeLocation(math_tools::Vector3D point) const
		{
			return point - getCenterOfMassLocation();
		}
		inline const math_tools::Vector3D & getTorque() const
		{
			return torque_;
		}

		// Setters:
		inline void setAngularMomentum(const math_tools::Vector3D & angular_momentum)
		{
			angular_momentum_ = angular_momentum;
		}
		inline void setAngularVelocity(const math_tools::Vector3D & angular_velocity)
		{
			matrix_tools::transform(inertia_tensor_, angular_velocity, angular_momentum_);
		}
		inline void setCoefficientOfDynamicFrictionFor(double coefficient_of_friction, int type_id)
		{
			coefficients_of_dynamic_friction_[type_id] = coefficient_of_friction;
		}
		inline void setCoefficientOfStaticFrictionFor(double coefficient_of_friction, int type_id)
		{
			coefficients_of_static_friction_[type_id] = coefficient_of_friction;
		}
		inline void setCoefficientOfRestitutionFor(double coefficient_of_restitution, int type_id)
		{
			coefficients_of_restitution_[type_id] = coefficient_of_restitution;
		}
		inline void setInertiaTensor(const math_tools::Matrix3D & inertia_tensor)
		{
			math_tools::matrix_tools::copyMatrix(inertia_tensor, inertia_tensor_);
		}
		inline void setInertiaTensorInverse(const math_tools::Matrix3D & inertia_tensor_inverse)
		{
			math_tools::matrix_tools::copyMatrix(inertia_tensor_inverse, inertia_tensor_inverse_);
		}
		inline void setCurrentOrientation(const math_tools::Matrix3D & current_orientation)
		{
			math_tools::matrix_tools::copyMatrix(current_orientation, current_orientation_);
		}

	protected:

		RigidBody(const RigidBody & rigid_body);
		RigidBody();

	private:
		// Full angular momentum of the object.
		math_tools::Vector3D angular_momentum_;

		// Elasticity coeffs.
		std::map<int, double> coefficients_of_restitution_;

		// Dynamic friction coeffs.
		std::map<int, double> coefficients_of_dynamic_friction_;

		// Static friction coeffs.
		std::map<int, double> coefficients_of_static_friction_;

		// Torque acting on the object in one time step.
		math_tools::Vector3D torque_;

		// Matrix which contains the current orientation of the object
		math_tools::Matrix3D current_orientation_;

		// Inertia tensor of the object.
		math_tools::Matrix3D inertia_tensor_;

		// Inverse of the inertia tensor, it is needed for computing
		// the collision handling.
		math_tools::Matrix3D inertia_tensor_inverse_;

		// Status of rigid body from previous frame.
		double status_rigid_body[22];
	};
}

#endif
